#ifndef ANDRES_BASE_CAMERA_H
#define ANDRES_BASE_CAMERA_H
#include <string>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <opencv2/opencv.hpp>

class Mounting
{
public:
	Eigen::Vector3d translation;	// x-forward, y-right, z-down
	Eigen::Quaterniond rotation;	// from FRD to image RDF frame
		
	Mounting(Eigen::Vector3d cam_ypr_offset = Eigen::Vector3d(0,0,0))
	{
		using namespace Eigen;

		translation.setZero(); 
		SetYpr(cam_ypr_offset);
	};
	
	void SetYpr(const Eigen::Vector3d &cam_ypr_offset)
	{
		rotation = QuatFromYpr(cam_ypr_offset) *
					GetQbody2camFix();
	};

	void GetYpr(Eigen::Vector3d *cam_ypr_offset) const
	{
		Eigen::Quaterniond q = GetQbody2camFix();
		(*cam_ypr_offset) = QuatToYpr(rotation * q.inverse());
		return;
	};

	static Eigen::Quaterniond GetQbody2camFix()
	{
		return QuatFromYpr(Eigen::Vector3d(M_PI / 2, 0, M_PI / 2));
	}

    template<typename T>
	static Eigen::Quaternion<T> QuatFromYpr(const Eigen::Matrix<T,3,1>& eular_angle)
    {
        using namespace Eigen;
		AngleAxis<T> yawQ(eular_angle.z(), Matrix<T,3,1>::UnitZ());
		AngleAxis<T> pitQ(eular_angle.y(), Matrix<T,3,1>::UnitY());
		AngleAxis<T> rolQ(eular_angle.x(), Matrix<T,3,1>::UnitX());
		Quaternion<T> q = yawQ * pitQ * rolQ;
		if(q.w() < 0)
		{
			q = QuatFlip(q);
		}
		return q;
    }

    template<typename T>
	static Eigen::Matrix<T,3,1> QuatToYpr(const Eigen::Quaternion<T>& q)
	{
		using namespace Eigen;
		Matrix<T,3,1> vec;
		T qw = q.w();
		T qx = q.x();
		T qy = q.y();
		T qz = q.z();
		vec[0] =  atan2(2*(qy*qz+qw*qx),  qw*qw - qx*qx - qy*qy + qz*qz);
		vec[1] = -asin(2*(qx*qz-qw*qy));
		vec[2] =  atan2(2*(qx*qy+qw*qz),  qw*qw + qx*qx - qy*qy - qz*qz);
		return vec;
	}

    template<typename T>
	static Eigen::Quaternion<T> QuatFlip(const Eigen::Quaternion<T> &q_in)
	{
		using namespace Eigen;
		Quaternion<T> q_out;
		q_out.w() = -q_in.w();
		q_out.x() = -q_in.x();
		q_out.y() = -q_in.y();
		q_out.z() = -q_in.z();
		return q_out;
	}
	
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

class Camera
{
public:
	unsigned int height;
	unsigned int width;
	Mounting mount;
	std::string distortion_model;
	double D[5];
	Eigen::Matrix3d K; // 3x3 row-major matrix
	double& fx() { return K(0,0); };
	double& fy() { return K(1,1); };
	double& cx() { return K(0,2); };
	double& cy() { return K(1,2); };
	const double& fx() const{ return K(0,0); };
	const double& fy() const{ return K(1,1); };
	const double& cx() const{ return K(0,2); };
	const double& cy() const{ return K(1,2); };
	
    bool LoadFromCfg(const std::string& cfg_file);
    cv::Mat get_K() const;
    cv::Mat get_D() const;

    cv::Vec2d DistoFunction(double u, double v) const;
    cv::Point2d AddDisto(double u, double v) const;
	cv::Point2d RemoveDisto(double u, double v) const;
	cv::Point2d RemoveDisto(const cv::Point2d & uv) const;
	cv::Point2d Pixel2Cam(const cv::Point2d & xi) const;
	cv::Point2d Cam2Pixel(const cv::Point2d & xc) const;
    bool Project3d_2d(Eigen::Vector3d pt3d, Eigen::Vector2d &img2D) const;
};

#endif